#include "lidar/classify/moving_object_detection.h"

namespace avos {
namespace perception {

MovingObjectDetection::MovingObjectDetection(double stamped_buffer_length,
                                             double latency)
    : time_latency_(latency),
      cells_with_pose_list_(ros::Duration(stamped_buffer_length)) {
  // FIXME: custom params
  grid_map_ = GridMapOccupancy<uint8_t>(20, 30, 0.1);
};

void MovingObjectDetection::Filter(ros::Time data_time,
                                   DummyVector<LidarObject>& lidar_objects,
                                   Rigid3<float> current_pose) {
  // reset
  accumulate_local_cells_.clear();

  // TIPS: check cloud queue, time roll back
  if (!cells_with_pose_list_.empty()) {
    ros::Time latest_cloud_time = cells_with_pose_list_.latest_time();
    if (data_time < latest_cloud_time) {
      ROS_ERROR_STREAM("[Lidar Noise Filter] input time roll back");
      cells_with_pose_list_.Clear();
    }
  }

  // get cloud lidar noise/tiny objects cells
  const uint8_t tiny_type_id =
      static_cast<uint8_t>(LidarObjectGroup::ObjectType::TINY);

  // clear grid map
  grid_map_.Reset();
  auto iter_begin = cells_with_pose_list_.rbegin();
  auto iter_end = cells_with_pose_list_.rend();
  Rigid3<float> base_pose = current_pose;
  Rigid3<float> base_pose_inverse = base_pose.inverse();
  //
  float grid_rx = 0.0, grid_ry = 0.0;
  int grid_x = -1, grid_y = -1;
  while (iter_begin != iter_end) {
    ros::Time latest_cloud_time = cells_with_pose_list_.latest_time();
    if ((latest_cloud_time - iter_begin->stamp_).toSec() > time_latency_) {
      Rigid3<float> local_pose = base_pose_inverse * iter_begin->data_.pose;
      for (const auto& cell : iter_begin->data_.cells) {
        Eigen::Vector3f trans_cell = local_pose * cell;
        grid_rx = -trans_cell[1];
        grid_ry = trans_cell[0];
        accumulate_local_cells_.push_back(std::move(trans_cell));
        grid_map_.GetIndex(grid_rx, grid_ry, grid_x, grid_y);
        if (grid_map_.InRange(grid_x, grid_y)) {
          // FIXME:
          grid_map_.Set(grid_x, grid_y, 1);
        }
      }
    }
    iter_begin++;
  }

  // collect TINY & NOISE cells
  DummyVector<Eigen::Vector3f> objects_cells;
  for (const LidarObject& object : lidar_objects) {
    if (object.type != tiny_type_id) {
      for (const LidarCell& cell : object.cells) {
        Eigen::Vector3f sensi_cell(cell.position[0], cell.position[1], 0.0);
        objects_cells.push_back(std::move(sensi_cell));
      }
    }
  }

  // push current cells list into cells array
  StampedStorage<CellsWithPose2> stamped_cells_with_pose(
      data_time, CellsWithPose2(objects_cells, base_pose));

  // check objects history occupancy
  DummyVector<int> objects_occupancies(lidar_objects.size(), -1);
  for (size_t obj_idx = 0; obj_idx < lidar_objects.size(); ++obj_idx) {
    LidarObject& object = lidar_objects[obj_idx];
    // TODO: specify the objects type
    if (object.type !=
        static_cast<uint8_t>(LidarObjectGroup::ObjectType::TINY)) {
      int occupancied_cell_count = 0;
      for (const LidarCell& cell : object.cells) {
        // there are history occupancy objects in grid view
        // so recover the tiny objects
        grid_rx = -cell.position[1];
        grid_ry = cell.position[0];
        grid_map_.GetIndex(grid_rx, grid_ry, grid_x, grid_y);
        if (grid_map_.InRange(grid_x, grid_y)) {
          // FIXME:
          if (grid_map_.Value(grid_x, grid_y) >= 1) {
            occupancied_cell_count++;
          }
        }
      }

      if (!object.cells.empty()) {
        float occupancied_percent =
            occupancied_cell_count * 1.0 / object.cells.size();
        // TODO: dynamic params
        if (occupancied_percent < 0.1) {
          object.dynamic_status =
              static_cast<uint8_t>(LidarObjectGroup::DynamicStatus::MOVING);
        } else if (occupancied_percent > 0.4) {
          object.dynamic_status =
              static_cast<uint8_t>(LidarObjectGroup::DynamicStatus::STATIC);
        }
      }
    }
  }

  // finally push latest sensitive cells input cell history list
  cells_with_pose_list_.Push(std::move(stamped_cells_with_pose));
}

void MovingObjectDetection::Visualize(
    pcl::PointCloud<pcl::PointXYZ>& cells_cloud) {
  for (auto& cell : accumulate_local_cells_) {
    pcl::PointXYZ point_xyz;
    point_xyz.x = cell[0];
    point_xyz.y = cell[1];
    point_xyz.z = -0.1;
    cells_cloud.points.push_back(point_xyz);
  }
}
}  // namespace perception
}  // namespace avos